#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/OccupancyGrid.h>
#include <tf/transform_listener.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>

class PointCloudMapper
{
public:
  PointCloudMapper() : 
    tf_buffer_(ros::Duration(10)),
    tf_listener_(tf_buffer_)
  {
    ros::NodeHandle nh;
    ros::NodeHandle private_nh("~");

    // 参数配置
    private_nh.param("map_resolution", map_resolution_, 0.05);
    private_nh.param("map_width", map_width_, 20.0);
    private_nh.param("map_height", map_height_, 20.0);
    private_nh.param("map_frame", map_frame_, std::string("map"));

    // 初始化地图
    initializeMap();

    // 订阅点云话题
    cloud_sub_ = nh.subscribe<sensor_msgs::PointCloud2>(
        "/camera/depth/points", 1, &PointCloudMapper::cloudCallback, this);

    // 发布地图话题
    map_pub_ = nh.advertise<nav_msgs::OccupancyGrid>("/output_map", 1);
  }

private:
  void initializeMap()
  {
    map_.header.frame_id = map_frame_;
    map_.info.resolution = map_resolution_;
    map_.info.width = static_cast<unsigned int>(map_width_ / map_resolution_);
    map_.info.height = static_cast<unsigned int>(map_height_ / map_resolution_);
    map_.info.origin.position.x = -map_width_ / 2.0;
    map_.info.origin.position.y = -map_height_ / 2.0;
    map_.info.origin.position.z = 0.0;
    map_.info.origin.orientation.w = 1.0;

    map_.data.resize(map_.info.width * map_.info.height, -1);
  }

  void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
  {
    try {
      // 转换坐标系
      // geometry_msgs::TransformStamped transform;
      // transform = tf_buffer_.lookupTransform(
      //     map_frame_, msg->header.frame_id,
      //     msg->header.stamp, ros::Duration(0.1));

      // 转换到PCL格式
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
      pcl::fromROSMsg(*msg, *cloud);

      // 坐标变换
      // pcl::PointCloud<pcl::PointXYZ> transformed_cloud;
      // pcl_ros::transformPointCloud(*cloud, transformed_cloud, transform.transform);

      // 更新地图
      // updateMap(transformed_cloud);
      updateMap(*cloud);//这里调试用先不要转换了
      // 发布地图
      map_.header.stamp = ros::Time::now();
      map_pub_.publish(map_);
    }
    catch (tf2::TransformException& ex) {
      ROS_WARN("TF转换失败: %s", ex.what());
    }
  }

  void updateMap(const pcl::PointCloud<pcl::PointXYZ>& cloud)
  {
    for (const auto& point : cloud) {
      int map_x = static_cast<int>((point.x - map_.info.origin.position.x) / map_resolution_);
      int map_y = static_cast<int>((point.y - map_.info.origin.position.y) / map_resolution_);

      if (map_x >= 0 && map_x < static_cast<int>(map_.info.width) &&
          map_y >= 0 && map_y < static_cast<int>(map_.info.height))
      {
        int index = map_y * map_.info.width + map_x;
        map_.data[index] = 100;  // 设置为占据
      }
    }
  }

  ros::Subscriber cloud_sub_;
  ros::Publisher map_pub_;
  tf2_ros::Buffer tf_buffer_;
  tf2_ros::TransformListener tf_listener_;

  nav_msgs::OccupancyGrid map_;
  double map_resolution_;
  double map_width_;
  double map_height_;
  std::string map_frame_;
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "pointcloud_mapper");
  PointCloudMapper mapper;
  ros::spin();
  return 0;
}